#ifndef __POSE_2D_H__
#define __POSE_2D_H__

#include <iostream>
using namespace std;

// 二维真实位姿
class Pose2D{
    public:
        Pose2D(): x(0), y(0){};
        Pose2D(float x, float y): x(x), y(y){};
        Pose2D(float x, float y, double yaw): x(x), y(y), yaw(yaw){};
        float x = 0.0;
        float y = 0.0;
        double yaw = 0.0;

        // 友元函数声明：重载 + 运算符 未处理yaw
        friend Pose2D operator+(const Pose2D& lhs, const Pose2D& rhs){
            return Pose2D(lhs.x + rhs.x, lhs.y + rhs.y);
        }
        
        // 友元函数声明：重载 - 运算符 未处理yaw
        friend Pose2D operator-(const Pose2D& lhs, const Pose2D& rhs){
            return Pose2D(lhs.x - rhs.x, lhs.y - rhs.y);
        }

        // 定义乘法运算符的实现：Pose2D * 常量值 未处理yaw
        friend Pose2D operator*(const Pose2D& pose, double scalar) {
            return Pose2D(pose.x * scalar, pose.y * scalar);
        }

        // 定义乘法运算符的实现：常量值 * Pose2D 未处理yaw
        friend Pose2D operator*(double scalar, const Pose2D& pose) {
            return Pose2D(pose.x * scalar, pose.y * scalar);
        }

        // 运算符重载：<< 操作符
        friend std::ostream& operator<<(std::ostream& os, const Pose2D& pose){
            os << "Pose2D(x: " << pose.x << ", y: " << pose.y << ", yaw: " << pose.yaw << ")";
            return os;
        }

};


#endif